/**
 * @file mpu6050.c
 * @author liufeng (liufeng@injoinic.com)
 * @brief mpu6050 driver
 * @version 0.1
 * @date 2024-11-13
 *
 * @copyright Copyright (c) 2024  injoinic
 *
 */

#include "bsp_mpu6050_driver.h"
#include "bsp_mpu6050_port.h"
#include "bsp_mpu6050_reg.h"
#include "stdio.h"
#include "string.h"

#ifdef USE_FULL_ASSERT
#define assert_failed(char, int)              \
    do {                                      \
        printf("Error:%s,%d\r\n", char, int); \
        while (1) {                           \
        }                                     \
    } while (0)
#define assert_param(expr) \
    if (!expr)             \
    assert_failed(__FILE__, __LINE__)
#else
#define assert_param(expr) ((void)0U)
#endif

/* ------------------------------------------------------------------------------------ */
/*                                         函数声明                                      */
/* ------------------------------------------------------------------------------------ */

void int_interrupt_callback(mpu6050_driver_typedef* mpu6050_instance);
void dma_completed_callback(mpu6050_driver_typedef* mpu6050_instance);

/* ------------------------------------------------------------------------------------ */
/*                                         函数定义                                      */
/* ------------------------------------------------------------------------------------ */

/**
 * @brief 软件复位
 * @param[in] mpu6050_instance mpu6050实例
 */
void mpu6050_sw_reset(mpu6050_driver_typedef* mpu6050_instance)
{
    mpu6050_iic_write_byte(
        mpu6050_instance->hi2c, MPU6050_IIC_ADDR, MPU_PWR_MGMT1_REG, 0x80, 10
    );
    os_delay(100);
    mpu6050_iic_write_byte(
        mpu6050_instance->hi2c, MPU6050_IIC_ADDR, MPU_PWR_MGMT1_REG, 0x00, 10
    );
}

/**
 * @brief 设置陀螺仪量程
 * @param[in] mpu6050_instance mpu6050实例
 * @param[in] fsr 0: 250°/s
 *                1: 500°/s
 *                2: 1000°/s
 *                3: 2000°/s
 * @return 0: 通信成功
 *         1：通信失败
 */
uint8_t mpu6050_set_gyro_fsr(mpu6050_driver_typedef* mpu6050_instance, uint8_t fsr)
{
    return mpu6050_iic_write_byte(
        mpu6050_instance->hi2c, MPU6050_IIC_ADDR, MPU_GYRO_CFG_REG, fsr << 3, 10
    );
}

/**
 * @brief 设置加速度量程
 * @param[in] mpu6050_instance mpu6050实例
 * @param[in] fsr 0: ±2g
 *                1: ±4g
 *                2: ±8g
 *                3: ±16g
 * @return 0: 通信成功
 *         1：通信失败
 */
uint8_t mpu6050_set_accel_fsr(mpu6050_driver_typedef* mpu6050_instance, uint8_t fsr)
{
    return mpu6050_iic_write_byte(
        mpu6050_instance->hi2c, MPU6050_IIC_ADDR, MPU_ACCEL_CFG_REG, fsr << 3, 10
    );
}

/**
 * @brief 设置数字低通滤波器频率
 * @param[in] mpu6050_instance mpu6050实例
 * @param[in] lpf 5 ~ 256
 * @return 0: 通信成功
 *         1：通信失败
 */
uint8_t mpu6050_set_lpf(mpu6050_driver_typedef* mpu6050_instance, uint16_t lpf)
{
    uint8_t dat;

    if (lpf >= 188)
        dat = 1;
    else if (lpf >= 98)
        dat = 2;
    else if (lpf >= 42)
        dat = 3;
    else if (lpf >= 20)
        dat = 4;
    else if (lpf >= 10)
        dat = 5;
    else
        dat = 6;

    return mpu6050_iic_write_byte(
        mpu6050_instance->hi2c, MPU6050_IIC_ADDR, MPU_CFG_REG, dat, 10
    );
}

/**
 * @brief 设置采样率
 * @param[in] mpu6050_instance mpu6050实例
 * @param[in] rate 见寄存器手册公式
 * @return 0: 通信成功
 *         1：通信失败
 */
uint8_t mpu6050_set_rate(mpu6050_driver_typedef* mpu6050_instance, uint16_t rate)
{
    uint8_t ret;

    if (rate < 4)
        rate = 4;
    else if (rate > 1000)
        rate = 1000;

    uint8_t dat = 1000 / rate - 1;
    ret = mpu6050_iic_write_byte(
        mpu6050_instance->hi2c, MPU6050_IIC_ADDR, MPU_SAMPLE_RATE_REG, dat, 10
    );
    if (ret != MPU6050_OK) {
        return MPU6050_TIMEOUT;
    }

    ret = mpu6050_set_lpf(mpu6050_instance, rate >> 1);
    if (ret != MPU6050_OK) {
        return MPU6050_TIMEOUT;
    }

    return MPU6050_OK;
}

/**
 * @brief 获取温度值
 * @param[in] mpu6050_instance mpu6050实例
 * @return 0: 通信成功
 *         1：通信失败
 */
uint8_t mpu6050_get_temperature(mpu6050_driver_typedef* mpu6050_instance)
{
    uint8_t dat[2];
    uint8_t ret = mpu6050_iic_mem_read(
        mpu6050_instance->hi2c, MPU6050_IIC_ADDR, MPU_TEMP_OUTH_REG, dat, 2, 10
    );

    if (ret == MPU6050_OK) {
        int16_t raw = ((uint16_t)dat[0] << 8) | dat[1];
        mpu6050_instance->data.temperature = 36.53f + (raw / 340.f);
    }

    return ret;
}

/**
 * @brief  获取陀螺仪值
 * @param[in] mpu6050_instance mpu6050实例
 * @return 0: 通信成功
 *         1：通信失败
 */
uint8_t mpu6050_get_gyroscope(mpu6050_driver_typedef* mpu6050_instance)
{
    uint8_t dat[6];
    uint8_t ret = mpu6050_iic_mem_read(
        mpu6050_instance->hi2c, MPU6050_IIC_ADDR, MPU_GYRO_XOUTH_REG, dat, 6, 10
    );

    if (ret == MPU6050_OK) {
        mpu6050_instance->data.gyro_x_raw = ((uint16_t)dat[0] << 8) | dat[1];
        mpu6050_instance->data.gyro_y_raw = ((uint16_t)dat[2] << 8) | dat[3];
        mpu6050_instance->data.gyro_z_raw = ((uint16_t)dat[4] << 8) | dat[5];

        mpu6050_instance->data.gx = mpu6050_instance->data.gyro_x_raw / 131.0;
        mpu6050_instance->data.gy = mpu6050_instance->data.gyro_y_raw / 131.0;
        mpu6050_instance->data.gz = mpu6050_instance->data.gyro_z_raw / 131.0;
    }

    return ret;
}

/**
 * @brief 获取加速度值
 * @param[in] mpu6050_instance mpu6050实例
 * @return 0: 通信成功
 *         1：通信失败
 */
uint8_t mpu6050_get_accelerometer(mpu6050_driver_typedef* mpu6050_instance)
{
    uint8_t dat[6];
    uint8_t ret = mpu6050_iic_mem_read(
        mpu6050_instance->hi2c, MPU6050_IIC_ADDR, MPU_ACCEL_XOUTH_REG, dat, 6, 10
    );

    if (ret == MPU6050_OK) {
        mpu6050_instance->data.accel_x_raw = ((uint16_t)dat[0] << 8) | dat[1];
        mpu6050_instance->data.accel_y_raw = ((uint16_t)dat[2] << 8) | dat[3];
        mpu6050_instance->data.accel_z_raw = ((uint16_t)dat[4] << 8) | dat[5];

        mpu6050_instance->data.ax = mpu6050_instance->data.accel_x_raw / 16384.0;
        mpu6050_instance->data.ay = mpu6050_instance->data.accel_y_raw / 16384.0;
        mpu6050_instance->data.az = mpu6050_instance->data.accel_z_raw / 16384.0;
    }

    return ret;
}

/**
 * @brief 获取全部采集值
 * @param[in] mpu6050_instance mpu6050实例
 * @return 0: 通信成功
 *         1：通信失败
 */
uint8_t mpu6050_get_all(mpu6050_driver_typedef* mpu6050_instance)
{
    uint8_t dat[14];
    uint8_t ret = mpu6050_iic_mem_read(
        mpu6050_instance->hi2c, MPU6050_IIC_ADDR, MPU_ACCEL_XOUTH_REG, dat, 14, 10
    );

    if (ret == MPU6050_OK) {
        mpu6050_instance->data.accel_x_raw = ((uint16_t)dat[0] << 8) | dat[1];
        mpu6050_instance->data.accel_y_raw = ((uint16_t)dat[2] << 8) | dat[3];
        mpu6050_instance->data.accel_z_raw = ((uint16_t)dat[4] << 8) | dat[5];

        mpu6050_instance->data.ax = mpu6050_instance->data.accel_x_raw / 16384.0;
        mpu6050_instance->data.ay = mpu6050_instance->data.accel_y_raw / 16384.0;
        mpu6050_instance->data.az = mpu6050_instance->data.accel_z_raw / 16384.0;

        int16_t raw = ((uint16_t)dat[6] << 8) | dat[7];
        mpu6050_instance->data.temperature = 36.53f + (raw / 340.f);

        mpu6050_instance->data.gyro_x_raw = ((uint16_t)dat[8] << 8) | dat[9];
        mpu6050_instance->data.gyro_y_raw = ((uint16_t)dat[10] << 8) | dat[11];
        mpu6050_instance->data.gyro_z_raw = ((uint16_t)dat[12] << 8) | dat[13];

        mpu6050_instance->data.gx = mpu6050_instance->data.gyro_x_raw / 131.0;
        mpu6050_instance->data.gy = mpu6050_instance->data.gyro_y_raw / 131.0;
        mpu6050_instance->data.gz = mpu6050_instance->data.gyro_z_raw / 131.0;
    }

    return ret;
}

/**
 * @brief 初始化函数
 * @param[in] mpu6050_instance mpu6050实例
 * @return 0: 通信成功
 *         1：通信失败
 */
uint8_t mpu6050_init(mpu6050_driver_typedef* mpu6050_instance)
{
    uint8_t id;

    mpu6050_periph_init();                                   // mcu相关外设初始化
    mpu6050_instance->pf_sw_reset(mpu6050_instance);         // mpu6050软件复位
    mpu6050_instance->pf_set_gyro_fsr(mpu6050_instance, 0);  // 陀螺仪传感器，±250dps
    mpu6050_instance->pf_set_accel_fsr(mpu6050_instance, 0); // 加速度传感器，±2g
    mpu6050_instance->pf_set_rate(mpu6050_instance, 50);     // 采样率，50Hz
    mpu6050_iic_write_byte(
        mpu6050_instance->hi2c, MPU6050_IIC_ADDR, MPU_USER_CTRL_REG, 0X00, 10
    ); // 关闭IIC主模式
    mpu6050_iic_write_byte(
        mpu6050_instance->hi2c, MPU6050_IIC_ADDR, MPU_FIFO_EN_REG, 0X00, 10
    ); // 关闭FIFO
    mpu6050_iic_read_byte(
        mpu6050_instance->hi2c, MPU6050_IIC_ADDR, MPU_DEVICE_ID_REG, &id, 10
    ); // 读取设备ID
    if (id != MPU6050_ID) {
        return MPU6050_EID;
    }
    mpu6050_iic_write_byte(
        mpu6050_instance->hi2c, MPU6050_IIC_ADDR, MPU_PWR_MGMT1_REG, 0x01, 10
    ); // 设置CLKSEL，PLL X轴为参考时钟
    mpu6050_iic_write_byte(
        mpu6050_instance->hi2c, MPU6050_IIC_ADDR, MPU_PWR_MGMT2_REG, 0x00, 10
    ); // 加速度与陀螺仪都工作
    mpu6050_iic_write_byte(
        mpu6050_instance->hi2c, MPU6050_IIC_ADDR, MPU_INTBP_CFG_REG, 0X00, 10
    ); // INT引脚高电平有效
    mpu6050_iic_write_byte(
        mpu6050_instance->hi2c, MPU6050_IIC_ADDR, MPU_INT_EN_REG, 0X01, 10
    ); // 打开data_ready中断

    return MPU6050_OK;
}

/**
 * @brief 逆初始化函数
 * @param[in] mpu6050_instance mpu6050实例
 * @return 0: 通信成功
 *         1：通信失败
 */
uint8_t mpu6050_deinit(mpu6050_driver_typedef* mpu6050_instance)
{
    mpu6050_instance->pf_sw_reset(mpu6050_instance); // mpu6050软件复位
    mpu6050_periph_deinit();                         // mcu相关外设逆初始化

    return MPU6050_OK;
}

/**
 * @brief 构造函数
 * @param[in] mpu6050_instance mpu6050实例
 * @param[in] hi2c i2c句柄(hal库或类似hal库)
 * @param[in] task_handle handler任务句柄，用于发送任务通知
 */
void mpu6050_construct(mpu6050_driver_typedef* mpu6050_instance, void* hi2c, void* task_handle)
{
    assert_param(hi2c);
    mpu6050_instance->pf_init = mpu6050_init;
    mpu6050_instance->pf_deinit = mpu6050_deinit;
    mpu6050_instance->pf_sw_reset = mpu6050_sw_reset;
    mpu6050_instance->pf_set_gyro_fsr = mpu6050_set_gyro_fsr;
    mpu6050_instance->pf_set_accel_fsr = mpu6050_set_accel_fsr;
    mpu6050_instance->pf_set_rate = mpu6050_set_rate;
    mpu6050_instance->pf_set_lpf = mpu6050_set_lpf;
    mpu6050_instance->pf_get_temperature = mpu6050_get_temperature;
    mpu6050_instance->pf_get_gyroscope = mpu6050_get_gyroscope;
    mpu6050_instance->pf_get_accelerometer = mpu6050_get_accelerometer;
    mpu6050_instance->pf_get_all = mpu6050_get_all;

    mpu6050_instance->hi2c = hi2c;
    mpu6050_instance->task_handle = task_handle;
    mpu6050_instance->unpack_queue = os_queue_create(5, sizeof(uint8_t*));
    mpu6050_instance->semaphore_mutex_handle = NULL;
    mpu6050_instance->pfdma_completed_callback = dma_completed_callback;
    mpu6050_instance->pfint_interrupt_callback = int_interrupt_callback;
    assert_param(mpu6050_instance->unpack_queue);

    memset(&(mpu6050_instance->data), 0, sizeof(mpu6050_instance->data));
    data_buff_construct(&mpu6050_instance->mpu6050_data_buff);
}

/* ------------------------------------------------------------------------------------ */
/*                                         中断回调                                      */
/* ------------------------------------------------------------------------------------ */

/**
 * @brief 外部中断回调函数
 * notes: 要确保mpu6050开启了中断功能
 *        代表mpu6050数据准备好，可以采集
 */
void int_interrupt_callback(mpu6050_driver_typedef* mpu6050_instance)
{
    // 关闭mpu6050中断
    mpu6050_iic_write_byte(mpu6050_instance->hi2c, MPU6050_IIC_ADDR, MPU_INT_EN_REG, 0X00, 10);
    // 查询mpu6050中断状态寄存器
    uint8_t int_status = 0;
    mpu6050_iic_read_byte(
        mpu6050_instance->hi2c, MPU6050_IIC_ADDR, MPU_INT_STA_REG, &int_status, 10
    );

    // 判断是否是data_ready中断
    if (int_status & 0x01) {
        // 切换到下一个缓冲区
        uint8_t* buff_addr = mpu6050_instance->mpu6050_data_buff.pf_write_addr(
            &mpu6050_instance->mpu6050_data_buff
        );
        if (buff_addr != NULL) {
            // 开启一次采样，把数据存到buffer缓冲区
            uint8_t ret = mpu6050_iic_mem_read_dma(
                mpu6050_instance->hi2c, MPU6050_IIC_ADDR, MPU_ACCEL_XOUTH_REG, buff_addr, 14
            );
        }
    }
}

/**
 * @brief DMA传输完成回调函数
 * notes: 此处未包含清中断和产生I2C Stop信号的操作
 *        代表mpu6050数据接收完了，可以进行处理
 */
void dma_completed_callback(mpu6050_driver_typedef* mpu6050_instance)
{
    // 开启mpu6050中断
    mpu6050_iic_write_byte(mpu6050_instance->hi2c, MPU6050_IIC_ADDR, MPU_INT_EN_REG, 0X01, 10);
    // 移动缓冲区的尾指针
    mpu6050_instance->mpu6050_data_buff.pf_move_tail(&mpu6050_instance->mpu6050_data_buff);
    // 通知handler_task进行处理
    os_task_notify_isr(mpu6050_instance->task_handle, NULL);
}
